#include <Arduino.h>

// 使用 UART2（RX2: GPIO16，TX2: GPIO17）
#define RXD2 16
#define TXD2 17

// 设置缓冲区大小
#define FRAME_SIZE 38   // 示例：帧头2字节 + 9个float (9*4=36字节)
uint8_t buffer[FRAME_SIZE];

// 工具函数：将4字节 IEEE754 转为 float
float bytesToFloat(uint8_t *bytes) {
  float value;
  memcpy(&value, bytes, sizeof(float));
  return value;
}

void setup() {
  Serial.begin(115200);          // 串口打印用
  Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);  // 与OPS-9通信串口
  Serial.println("OPS-9 数据解析启动...");
}

void loop() {
  // 查找帧头 0xAA 0x55
  if (Serial2.available() >= FRAME_SIZE) {
    int header1 = Serial2.read();
    if (header1 == 0xAA) {
      int header2 = Serial2.read();
      if (header2 == 0x55) {
        // 读取剩余数据
        Serial2.readBytes(buffer, FRAME_SIZE - 2);

        // 解析数据帧中的 float
        float pitch  = bytesToFloat(&buffer[0]);
        float roll   = bytesToFloat(&buffer[4]);
        float yaw    = bytesToFloat(&buffer[8]);
        float x_pos  = bytesToFloat(&buffer[12]);
        float y_pos  = bytesToFloat(&buffer[16]);
        float vx     = bytesToFloat(&buffer[20]);
        float vy     = bytesToFloat(&buffer[24]);
        float gz     = bytesToFloat(&buffer[28]);  // 航向角速度（Z轴）
        float temp   = bytesToFloat(&buffer[32]);  // 例如：温度或其他值

        // 打印解析数据
        Serial.println("----- 接收到数据帧 -----");
        Serial.printf("角度(Pitch, Roll, Yaw): %.2f, %.2f, %.2f\n", pitch, roll, yaw);
        Serial.printf("位置(X, Y): %.2f, %.2f\n", x_pos, y_pos);
        Serial.printf("航向角速度 Gz: %.2f deg/s\n", gz);
        Serial.println("-----------------------");
      }
    }
  }
}